/*
Arm Tracker and Glove Demo by Noah Zerkin (noazark AT gmail.com) 2009
Released under GNU GPLv3.0 http://www.opensource.org/licenses/gpl-3.0.html
*/

int correctVal(int val){
  if(val <= 60){
    return int(map(val, 59, 1, 285, 340));
  }
  else{
    return int(map(val, 255, 200, 341, 400));
  }
}

int correctZVal(int val){
  if(val <= 60){
    return int(map(val, 59, 1, 400, 341));
  }
  else{
    return int(map(val, 255, 200, 340, 285));
  }
}

int correctGloveVal(int val){
  if(val <= 60){
    return int(map(val, 59, 1, 285, 340));
  }
  else{
    return int(map(val, 255, 200, 341, 400));
  }
}

int correctGloveZVal(int val){
  if(val <= 60){
    return int(map(val, 59, 1, 400, 341));
  }
  else{
    return int(map(val, 255, 200, 340, 285));
  }
}

void parseAndFilter(int s){
  switch(s){
  case 0:
    xRaw[s] = correctZVal(buffX[s]);
    zRaw[s] = correctVal(buffY[s]);
    yRaw[s] = correctVal(buffZ[s]);
    break;
  case 1:
    xRaw[s] = correctZVal(buffX[s]);
    zRaw[s] = correctZVal(buffY[s]);
    yRaw[s] = correctVal(buffZ[s]);
    break;
  }
  gxRaw[s] = buffGX[s];// print(gxRaw[s]); print(" ");
  gyRaw[s] = buffGY[s];// println(gyRaw[s]);
  flexRaw[s] = buffFlex[s];
  gloveXRaw[s] = correctGloveVal(buffGloveX[s]);
  gloveYRaw[s] = correctGloveVal(buffGloveY[s]);
  gloveZRaw[s] = correctGloveZVal(buffGloveZ[s]);

  /*
  accelFilter[s][0][filterCycle[s]] = xRaw[s];
   accelFilter[s][1][filterCycle[s]] = yRaw[s];
   accelFilter[s][2][filterCycle[s]] = zRaw[s];
   accelFilter[s][3][filterCycle[s]] = flexRaw[s];
   accelFilter[s][4][filterCycle[s]] = buffGloveX[s];
   accelFilter[s][5][filterCycle[s]] = buffGloveY[s];
   accelFilter[s][6][filterCycle[s]] = buffGloveZ[s];
   xVal[s] = (accelFilter[s][0][0] + accelFilter[s][0][1] + accelFilter[s][0][2] + accelFilter[s][0][3] + accelFilter[s][0][4] + accelFilter[s][0][5] + accelFilter[s][0][6] + accelFilter[s][0][7] + accelFilter[s][0][8] + accelFilter[s][0][9] + accelFilter[s][0][10] + accelFilter[s][0][11] + accelFilter[s][0][12] + accelFilter[s][0][13] + accelFilter[s][0][14] + accelFilter[s][0][15] + accelFilter[s][0][16] + accelFilter[s][0][17] + accelFilter[s][0][18] + accelFilter[s][0][19])/20;
   yVal[s] = (accelFilter[s][1][0] + accelFilter[s][1][1] + accelFilter[s][1][2] + accelFilter[s][1][3] + accelFilter[s][1][4] + accelFilter[s][1][5] + accelFilter[s][1][6] + accelFilter[s][1][7] + accelFilter[s][1][8] + accelFilter[s][1][9] + accelFilter[s][1][10] + accelFilter[s][1][11] + accelFilter[s][1][12] + accelFilter[s][1][13] + accelFilter[s][1][14] + accelFilter[s][1][15] + accelFilter[s][1][16] + accelFilter[s][1][17] + accelFilter[s][1][18] + accelFilter[s][1][19])/20;
   zVal[s] = (accelFilter[s][2][0] + accelFilter[s][2][1] + accelFilter[s][2][2] + accelFilter[s][2][3] + accelFilter[s][2][4] + accelFilter[s][2][5] + accelFilter[s][2][6] + accelFilter[s][2][7] + accelFilter[s][2][8] + accelFilter[s][2][9] + accelFilter[s][2][10] + accelFilter[s][2][11] + accelFilter[s][2][12] + accelFilter[s][2][13] + accelFilter[s][2][14] + accelFilter[s][2][15] + accelFilter[s][2][16] + accelFilter[s][2][17] + accelFilter[s][2][18] + accelFilter[s][2][19])/20;
   flexVal[s] =(accelFilter[s][3][0] + accelFilter[s][3][1] + accelFilter[s][3][2]   + accelFilter[s][3][3] + accelFilter[s][3][4] + accelFilter[s][3][5] + accelFilter[s][3][6] + accelFilter[s][3][7] + accelFilter[s][3][8] + accelFilter[s][3][9] + accelFilter[s][3][10] + accelFilter[s][3][11] + accelFilter[s][3][12]   + accelFilter[s][3][13] + accelFilter[s][3][14] + accelFilter[s][3][15] + accelFilter[s][3][16] + accelFilter[s][3][17] + accelFilter[s][3][18] + accelFilter[s][3][19])/20;
   gloveX[s] = (accelFilter[s][4][0] + accelFilter[s][4][1] + accelFilter[s][4][2] + accelFilter[s][4][3] + accelFilter[s][4][4] + accelFilter[s][4][5] + accelFilter[s][4][6] + accelFilter[s][4][7] + accelFilter[s][4][8] + accelFilter[s][4][9] + accelFilter[s][4][10] + accelFilter[s][4][11] + accelFilter[s][4][12] + accelFilter[s][4][13] + accelFilter[s][4][14] + accelFilter[s][4][15] + accelFilter[s][4][16] + accelFilter[s][4][17] + accelFilter[s][4][18] + accelFilter[s][4][19])/20;
   gloveY[s] = (accelFilter[s][5][0] + accelFilter[s][5][1] + accelFilter[s][5][2] + accelFilter[s][5][3] + accelFilter[s][5][4] + accelFilter[s][5][5] + accelFilter[s][5][6] + accelFilter[s][5][7] + accelFilter[s][5][8] + accelFilter[s][5][9] + accelFilter[s][5][10] + accelFilter[s][5][11] + accelFilter[s][5][12] + accelFilter[s][5][13] + accelFilter[s][5][14] + accelFilter[s][5][15] + accelFilter[s][5][16] + accelFilter[s][5][17] + accelFilter[s][5][18] + accelFilter[s][5][19])/20;
   gloveZ[s] = (accelFilter[s][6][0] + accelFilter[s][6][1] + accelFilter[s][6][2] + accelFilter[s][6][3] + accelFilter[s][6][4] + accelFilter[s][6][5] + accelFilter[s][6][6] + accelFilter[s][6][7] + accelFilter[s][6][8] + accelFilter[s][6][9] + accelFilter[s][6][10] + accelFilter[s][6][11] + accelFilter[s][6][12] + accelFilter[s][6][13] + accelFilter[s][6][14] + accelFilter[s][6][15] + accelFilter[s][6][16] + accelFilter[s][6][17] + accelFilter[s][6][18] + accelFilter[s][6][19])/20;
   if (filterCycle[s] < 19){
   filterCycle[s] += 1;
   }
   else{
   filterCycle[s] = 0;
   }
   */
  xVal[s] = xFilter.update(xRaw[s]);
  yVal[s] = yFilter.update(yRaw[s]);
  zVal[s] = zFilter.update(zRaw[s]);
  flexVal[s] = flexFilter.update(flexRaw[s]);
  gloveXVal[s] = gloveXFilter.update(gloveXRaw[s]);
  gloveYVal[s] = gloveYFilter.update(gloveYRaw[s]);
  gloveZVal[s] = gloveZFilter.update(gloveZRaw[s]);
  /*
  magX[s] = magXFilter.update(magXRaw[s]);
  magY[s] = magYFilter.update(magYRaw[s]);
  magZ[s] = magZFilter.update(magZRaw[s]);
  */
  magX[s] = magXRaw[s];
  magY[s] = magYRaw[s];
  magZ[s] = magZRaw[s];
}

void IMUcalc(int s){
  //  println(s);
  //  if(s == 0){
  lastFlex[s] = flex[s];
  flex[s] = radians(map(flexVal[s], flex180[s], flex90[s], 0, 90));
  elbowAngle[s] = (-flex[s])+PI;
  xa[s] = xb + (xm*xVal[s]);
  ya[s] = yb + (ym*yVal[s]);
  za[s] = zb + (zm*zVal[s]);
  xRad[s] = atan2(xa[s],sqrt(ya[s]*ya[s] + za[s]*za[s]));
  yRad[s] = atan2(ya[s],sqrt(xa[s]*xa[s] + za[s]*za[s]));
  zTilt[s] = atan2(za[s],sqrt(ya[s]*ya[s] + xa[s]*xa[s]));
  //  accelRawMap(s);
  accelQuadrantMap(s);
  // gyro
  switch(s){
  case 1:
    gx[s] = radians(map(gxRaw[s], gxZero[s], gxZero[s]+gyroScaleX[s], 0, -90));
    gy[s] = radians(map(gyRaw[s], gyZero[s], gyZero[s]+gyroScaleY[s], 0, 90));
    break;
  case 0:
    gx[s] = radians(map(gxRaw[s], gxZero[s], gxZero[s]+gyroScaleX[s], 0, -4.5));
    gy[s] = radians(map(gyRaw[s], gyZero[s], gyZero[s]+gyroScaleY[s], 0, 2));
    break;
  }
  gyroRotation(s);
  //  }
}

void gyroRotation(int s){
  if(abs(gx[s])>0.0001){
    if(abs(gy[s])>0.0001){
      //     println("gyro input");
      gxGy[s] = sqrt(sq(gx[s])+sq(gy[s]));
      //    gxGy = atan2(gx,gy);
      if(gx[s]<=0){
        gxGyTheta[s] = TWO_PI - acos(gy[s]/gxGy[s]);
      }
      else{
        gxGyTheta[s] = acos(gy[s]/gxGy[s]);
      }
      switch(s){
      case 1:
        yRot[s] -= (cos(gxGyTheta[s]-yTilt[s])*gxGy[s])*(((float)timeDelta[s])/1000);
        break;
      case 0:
        yRot[s] += (cos(gxGyTheta[s]-yTilt[s])*gxGy[s])*(((float)timeDelta[s])/1000);
        break;
      }
    }
  }
  //  print(yRot[s]); println(s);
}

void accelQuadrantMap(int s){
  // xTilt
  if (xRad[s]<0){
    if (yRad[s]<0){
      xTilt[s] = TWO_PI + xRad[s];
    }
    else{
      xTilt[s] = PI - xRad[s];
    }
  }
  if (xRad[s]>0){
    if (yRad[s]<0){
      xTilt[s] = xRad[s];
    }
    else{
      xTilt[s] = PI - xRad[s];
    }
  }
  // yTilt
  if (yRad[s]<0){
    if (sqrt(sq(xRad[s])+sq(zTilt[s]))<0){
      yTilt[s] = TWO_PI + yRad[s];
    }
    else{
      yTilt[s] = PI - yRad[s];
    }
  }
  if (yRad[s]>0){
    if (sqrt(sq(xRad[s])+sq(zTilt[s]))<0){
      yTilt[s] = yRad[s];
    }
    else{
      yTilt[s] = PI - yRad[s];
    }
  }
}

void accelRawMap(int s){
  xTilt[s] = xRad[s];
  yTilt[s] = yRad[s];
}

class FilterElement{
  int elementValue;

  FilterElement(int value){
    elementValue = value;
  }

  int getValue(){
    return elementValue;
  }

}

class WeightedFilter{

  ArrayList filterArray;
  float filterTotal = 0.0;
  int divisor = 0;
  float filterPool = 0.0;

  WeightedFilter(){
    filterArray = new ArrayList();
  }

  float update(int newest){
    if(filterArray.size() >= 1){
      FilterElement lastElement = (FilterElement)filterArray.get(filterArray.size()-1);
      if(newest != lastElement.getValue()){
        FilterElement newElement = new FilterElement(newest);
        filterArray.add(newElement);
        filterTotal = 0.0;
        filterPool = 0.0;
        divisor = 0;
        for(int i = 1; i < filterArray.size(); i++){
          FilterElement readElement = (FilterElement) filterArray.get(i);
          filterPool += readElement.getValue()*i;
          divisor += i;
        }
        filterTotal = filterPool / divisor;
        if(filterArray.size() >= 25){
          filterArray.remove(0);
        }
      }
      /*
            print(filterTotal);
      print(" - ");
      println(filterArray.size());
      */
      return filterTotal;
    }
    else{
      FilterElement initElement = new FilterElement(newest);
      filterArray.add(initElement);
      return float(newest);
    }
  }
  /*
  float getValue(){
   float filterTotal = 0.0;
   for(int i = 0; i < filterArray.size(); i++){
   filterTotal += float(filterArray.get(i))/(filterArray.size() - i);
   }
   if(filterArray.size() >= 10){
   filterArray.remove(0);
   }
   }
   */
}
